#include "Kinect.h"

// Note: wont work as expected for > 5 users in scene
static unsigned int g_UsersColors[] = {/*0x70707080*/0 ,0x80FF0000,0x80FF4500,0x80FF1493,0x8000ff00, 0x8000ced1,0x80ffd700};
#define GetColorForUser(i) g_UsersColors[(i)%(sizeof(g_UsersColors)/sizeof(unsigned int))]

#define VALIDATE_GENERATOR(type, desc, generator)				\
{																\
	rc = m_Context.EnumerateExistingNodes(nodes, type);			\
	if (nodes.IsEmpty())										\
{															\
	printf("No %s generator!\n", desc);						\
	return 1;												\
}															\
	(*(nodes.Begin())).GetInstance(generator);					\
}
#define CHECK_RC(rc, what)											\
	if (rc != XN_STATUS_OK)											\
{																\
	printf("%s failed: %s\n", what, xnGetStatusString(rc));		\
	return rc;													\
}


Kinect* Ogre::Singleton<Kinect>::ms_Singleton = 0;
Kinect g_singleTonKinect;

Kinect::Kinect():
	m_hUserCallbacks(NULL),
	m_hPoseCallbacks(NULL),
	m_hCalibrationCallbacks(NULL),
	isPause(false),
	isLost(false),
	m_front(false)
{
//	isHandTracking[MAX_HAND_NUM]={0};
}

Kinect::~Kinect()
{
	m_Context.StopGeneratingAll();

	if (NULL != m_hPoseCallbacks)
	{
		m_UserGenerator.GetPoseDetectionCap().UnregisterFromPoseCallbacks(m_hPoseCallbacks);
		m_hPoseCallbacks = NULL;
	}
	if (NULL != m_hUserCallbacks)
	{
		m_UserGenerator.UnregisterUserCallbacks(m_hUserCallbacks);
		m_hUserCallbacks = NULL;
	}
	if (NULL != m_hCalibrationCallbacks)
	{
		m_UserGenerator.GetSkeletonCap().UnregisterCalibrationCallbacks(m_hCalibrationCallbacks);
		m_hCalibrationCallbacks = NULL;
	}

	m_Context.Shutdown();
}

XnStatus Kinect::BootKinect()
{
	rc = m_Context.InitFromXmlFile(".\\Data\\openni.xml");
	CHECK_RC(rc, "InitFromXml");

	// Make sure we have all OpenNI nodes we will be needing for this sample
	xn::NodeInfoList nodes;

	VALIDATE_GENERATOR(XN_NODE_TYPE_USER, "User", m_UserGenerator);
	VALIDATE_GENERATOR(XN_NODE_TYPE_HANDS, "Gesture", m_GestureGenerator);
	VALIDATE_GENERATOR(XN_NODE_TYPE_HANDS, "Hands", m_HandsGenerator);
		
	// Init NITE Controls (UI stuff)
	m_pSessionManager = new XnVSessionManager;
	rc = m_pSessionManager->Initialize(&m_Context, "Click", "RaiseHand");
	m_pSessionManager->SetQuickRefocusTimeout(0);

	// Create quit slider & add to session manager
	m_pQuitSSlider = new XnVSelectableSlider1D(1,0,AXIS_X,1,0,500);
	m_pQuitSSlider->RegisterPrimaryPointCreate(this,Kinect::quitSSliderPPC);
	m_pQuitSSlider->RegisterPrimaryPointDestroy(this,Kinect::quitSSliderPPD);
	m_pQuitSSlider->RegisterOffAxisMovement(this,Kinect::quitSSliderOAM);
	m_pQuitSSlider->RegisterValueChange(this,Kinect::quitSSliderVC);
		
	m_pQuitFlow = new XnVFlowRouter();
	m_pQuitFlow->SetActive(m_pQuitSSlider);		
	m_pSessionManager->AddListener(m_pQuitFlow);

	suppress = false;

	// Init OpenNI nodes
	m_HandsGenerator.SetSmoothing(1);
	m_UserGenerator.RegisterUserCallbacks(Kinect::NewUser, Kinect::LostUser, this, m_hUserCallbacks);
	m_UserGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(Kinect::PoseDetected, Kinect::PoseLost, this, m_hPoseCallbacks);

	// Skeleton stuff
	m_SmoothingFactor = 0.6;
	m_SmoothingDelta = 0;
	xn::SkeletonCapability skel = m_UserGenerator.GetSkeletonCap();
	skel.SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
	skel.SetSmoothing(m_SmoothingFactor);
	skel.RegisterCalibrationCallbacks(Kinect::CalibrationStart, Kinect::CalibrationEnd, this, m_hCalibrationCallbacks);

	//gesture
	m_HandsGenerator.Create(m_Context);
	m_GestureGenerator.Create( m_Context );

	m_GestureGenerator.AddGesture( "Wave", NULL );
	m_GestureGenerator.AddGesture( "Click", NULL );
//	m_GestureGenerator.AddGesture( "RaiseHand", NULL );
//	m_GestureGenerator.AddGesture( "MovingHand", NULL );
	
	m_GestureGenerator.RegisterGestureCallbacks( Kinect::gestureRecog, Kinect::gestureProgress, this, m_hGestureCallbacks );
	m_HandsGenerator.RegisterHandCallbacks(Kinect::Hand_Create, Kinect::Hand_Update, Kinect::Hand_Destroy, this, m_hHandCallbacks);

	// Make sure OpenNI nodes start generating
	rc = m_Context.StartGeneratingAll();
	CHECK_RC(rc, "StartGenerating");

	m_candidateID = 0;
	m_pStartPoseDetector = new StartPoseDetector(3.0);
	m_pEndPoseDetector = new EndPoseDetector(m_UserGenerator, 2.0);
	m_pEndPoseDetector->SetUserId(m_candidateID);

	return XN_STATUS_OK;
}

void Kinect::initPrimeSensor()
{
	rc = BootKinect();
	if (XN_STATUS_OK != rc)
	{
		Ogre::ErrorDialog dlg;
		dlg.display("initializing Kinect Device Error, Check Your Connection Please! Now Entering PC Mode, Enjoy Yourself!");
		isDown = true;
	}
}




Ogre::Quaternion Kinect::getJointOrientation(XnSkeletonJoint skelJoint)
{
	Ogre::Quaternion newQ = Ogre::Quaternion::IDENTITY;

	// Get the openNI bone info
	xn::SkeletonCapability pUserSkel = m_UserGenerator.GetSkeletonCap();		
	XnSkeletonJointOrientation jointOri;
	pUserSkel.GetSkeletonJointOrientation(m_candidateID, skelJoint, jointOri);

	if(jointOri.fConfidence > 0 )
	{
		XnVector3D col1 = xnCreatePoint3D(jointOri.orientation.elements[0], jointOri.orientation.elements[3], jointOri.orientation.elements[6]);
		XnVector3D col2 = xnCreatePoint3D(jointOri.orientation.elements[1], jointOri.orientation.elements[4], jointOri.orientation.elements[7]);
		XnVector3D col3 = xnCreatePoint3D(jointOri.orientation.elements[2], jointOri.orientation.elements[5], jointOri.orientation.elements[8]);
	
		Ogre::Matrix3 matOri(jointOri.orientation.elements[0],-jointOri.orientation.elements[1],jointOri.orientation.elements[2],
							-jointOri.orientation.elements[3],jointOri.orientation.elements[4],-jointOri.orientation.elements[5],
							jointOri.orientation.elements[6],-jointOri.orientation.elements[7],jointOri.orientation.elements[8]);
		Ogre::Quaternion q;
			
		newQ.FromRotationMatrix(matOri);
		
	} 
	return newQ;
}

Ogre::Vector3 Kinect::getJointPos(XnSkeletonJoint skelJoint)
{
	Ogre::Vector3 result;
	m_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(m_candidateID, skelJoint, jointPos);
	result.x = jointPos.position.X;
	result.y = jointPos.position.Y;
	result.x = jointPos.position.Z;
	return result;
}

void Kinect::Pause()
{
	isPause = true;
}

void Kinect::Restart()
{
	isPause = false;
}